/**
* @file SerialThread.cpp in RobotConrol
* @author rebeater
* @comment
* Create on 11/6/21 3:41 PM
* @version 1.0
**/


#include "SerialThread.h"
#include <glog/logging.h>
#include <iostream>
SerialThread::SerialThread(QObject *parent) : QThread(parent) {
  flag_stop_.test_and_set();
}
bool SerialCheckSum(uint8_t data[], int start, int end) {
  uint8_t checkSum = 0;
  for (int i = start; i <= end; i++) {
	checkSum += data[i];
  }
  return true;
};
bool SerialThread::Stop() {
  flag_stop_.clear();
  return true;
}
bool SerialThread::DecodeData(uint8_t data[], int length) {
  RobotMessage robotmsg;
  for (int i = 0; i < length - 2; i++) {
	/*查找数据头*/
	if ((data[i] == 0x5A) and (data[i + 1] == (uint8_t)0xA5)) {
	  robotmsg.length = (int)data[i + 2];
	  robotmsg.type_ = (RobotMessageType)data[i + 3];
	  robotmsg.id_ = (RobotMessageID)data[i + 4];
	  /*判断数据完整性*/
	  if (i + robotmsg.length + 8 > length) {
		LOG(ERROR) << "Data is not complete";
		return false;
	  }
	  /*校验*/
	  if (SerialCheckSum(data, i, i + robotmsg.length + 8)) {
		/*复制数据*/
		i += 5;
		for (int j = 0; j < robotmsg.length; j++) {
		  robotmsg.data[j] = data[i];
		  i++;
		}
		/*通知UI*/
		emit dataReadyToUpdateUi(robotmsg);
	  }
	  return false;
	}
  }
  return false;
}
void SerialThread::run() {
//  QThread::run();
  LOG(INFO) << "Serial read and write thread start";
  std::cout << "SerialThread";
  QSerialPort serial;
  serial.setPortName(dev_);
  serial.setBaudRate(baud_rate_);
//  serial.tim
  serial.setDataBits(QSerialPort::Data8);
  serial.setFlowControl(QSerialPort::NoFlowControl);
  serial.setParity(QSerialPort::NoParity);
  serial.open(QSerialPort::ReadWrite);
   if (!serial.isOpen()) {
	LOG(ERROR) << "Open failed: " << qt_error_string(serial.error()).toStdString();
	serial.close();
	serialPortStatus(false);
	return;
  }
  serialPortStatus(true);
  LOG(INFO) << dev_.toStdString() << " open success";

  /*请求版本信息*/

  uint8_t req[8] = {0x5A, 0xA5, 0x05, 0x55, 0x05, 0x01, 0x01, 0x00};
  uint8_t *cmd_idx = &req[6];
  for (int i = 0; i < 7; i++) req[7] += req[i];
  serial.write((const char *)req, 8);
  uint8_t buff[256];
  while (true) {
	if (serial.waitForReadyRead(500)) {
//	  QByteArray data = serial.read(256);
	  int length = serial.read((char *)buff, 256);
	  if (length > 0) {
		DecodeData(buff, length);
		msleep(25);
	  }
	}
	if (!serial.isReadable()) {
	  LOG(INFO) << "serial port is not readable";
	  break;
	}
	if (!flag_stop_.test_and_set()) {
	  break;
	}
  }
  LOG(INFO) << "Close serial port";
  serialPortStatus(false);
}
void SerialThread::SetPortAndRate(QString &dev, int baud_rate) {
  dev_ = dev;
  baud_rate_ = baud_rate;
}

